#include <string.h>
#include <stdio.h>
#include "common.h"
#include "imu.h"
#include "stm32f4xx.h"

#ifndef ARRAY_SIZE
#define ARRAY_SIZE(x)	(sizeof(x) / sizeof((x)[0]))
#endif

#ifndef CH_OK
#define CH_OK   (0)
#endif

#ifndef CH_ERR
#define CH_ERR  (1)
#endif

#define IMU_BAUDRATE		115200
#define IMU_TX_PIN			GPIO_Pin_6
#define IMU_TX_PinSource    GPIO_PinSource6

#define IMU_RX_PIN			GPIO_Pin_7
#define IMU_RX_PinSource    GPIO_PinSource7

#define	IMU_PORT			GPIOC
#define	IMU_IO_CLK_EN		RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE)

#define IMU_UART			USART6
#define IMU_UART_CLK_EN		RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART6, ENABLE)

#define IMU_GPIO_AF_UART 		GPIO_AF_USART6

static int16_t acc[3];
static int16_t gyo[3];
static int16_t mag[3];
static float eular[3];
static float quat[4];
static uint8_t id;
static uint8_t eulars[6];

uint8_t imu_dma_rx_data[MAX_IMU_PACKET_LEN] = {0};
uint16_t imu_recv_len = 0;

static void DMAIMURx_Init(void)
{
    DMA_InitTypeDef   DMA_InitStructure;

    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE);                        //启动DMA时钟

    DMA_DeInit(DMA2_Stream1);

    DMA_InitStructure.DMA_Channel = DMA_Channel_5;
    DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)(&USART6->DR);         //外设地址
    DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)imu_dma_rx_data;          //内存地址
    DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;                     //dma传输方向单向
    DMA_InitStructure.DMA_BufferSize = MAX_IMU_PACKET_LEN;                          //设置DMA在传输时缓冲区的长度
    DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;            //设置DMA的外设递增模式，一个外设
    DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;                     //设置DMA的内存递增模式
    DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;     //外设数据字长
    DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;             //内存数据字长
    DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;                               //设置DMA的传输模式
    DMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh;                     //设置DMA的优先级别

 	DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
 	DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
 	DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
 	DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;

//    DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;                                //设置DMA的2个memory中的变量互相访问
    DMA_Init(DMA2_Stream1,&DMA_InitStructure);
    DMA_Cmd(DMA2_Stream1,ENABLE);                                               //使能数据流1 通道5
}

void IMU_Init()
{
	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;

	IMU_UART_CLK_EN;
	IMU_IO_CLK_EN;

	GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_UP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

	GPIO_InitStructure.GPIO_Pin = IMU_TX_PIN;
	GPIO_Init(IMU_PORT, &GPIO_InitStructure);
	GPIO_InitStructure.GPIO_Pin = IMU_RX_PIN;
	GPIO_Init(IMU_PORT, &GPIO_InitStructure);

	GPIO_PinAFConfig(IMU_PORT,IMU_TX_PinSource,IMU_GPIO_AF_UART);
	GPIO_PinAFConfig(IMU_PORT,IMU_RX_PinSource,IMU_GPIO_AF_UART);

	USART_InitStructure.USART_BaudRate 	 = IMU_BAUDRATE;
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;
	USART_InitStructure.USART_StopBits   = USART_StopBits_1;
	USART_InitStructure.USART_Parity     = USART_Parity_No ;
	USART_InitStructure.USART_Mode 	    = USART_Mode_Rx | USART_Mode_Tx;
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	USART_Init(IMU_UART,&USART_InitStructure);

	NVIC_InitStructure.NVIC_IRQChannel = USART6_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStructure);

	USART_ITConfig(IMU_UART,USART_IT_TC,DISABLE);
	USART_ITConfig(IMU_UART,USART_IT_RXNE,DISABLE);
	USART_ITConfig(IMU_UART,USART_IT_TXE,DISABLE);
	USART_ITConfig(IMU_UART,USART_IT_IDLE,ENABLE);

	DMAIMURx_Init();
	USART_DMACmd(IMU_UART,USART_DMAReq_Rx,ENABLE);
	USART_Cmd(IMU_UART,ENABLE);
}

uint32_t Packet_CreatePing(Packet_t *pkt)
{
    pkt->buf[0] = 0x5A;
    pkt->buf[1] = 0xA6;
    pkt->payload_len = 0;
    pkt->len = 2;
    return CH_OK;
}

uint32_t Packet_CreatePingAck(Packet_t *pkt, uint8_t major, uint8_t minor, uint8_t bugfix, uint16_t option)
{
    pkt->buf[0] = 0x5A;
    pkt->buf[1] = 0xA7;

    /* protocol bug fix */
    pkt->buf[2] = bugfix;
    /* protocol minor */
    pkt->buf[3] = minor;
    /* protocol major */
    pkt->buf[4] = major;
    pkt->buf[5] = 'P';

    /* option low: sender's address low */
    pkt->buf[6] = (option & 0x00FF)>>0;

    /* option high: sender's address high */
    pkt->buf[7] = (option & 0xFF00)>>8;

    /* crc */
    uint16_t crc;
    crc = 0;
    crc16_update(&crc, &pkt->buf[0], 8);
    pkt->buf[8] = (crc & 0x00FF)>>0;
    pkt->buf[9] = (crc & 0xFF00)>>8;

    pkt->payload_len = 0;
    pkt->type = 0xA7;
    pkt->len = 10;
    return CH_OK;
}


uint32_t Packet_Begin(Packet_t *pkt)
{
    pkt->ofs = 6; /* sof(2) len(2) + crc(2) */
    memset(&pkt->buf[0], 0, sizeof(pkt->buf));
    pkt->buf[0] = 0x5A; /* header */
    pkt->buf[1] = 0xA5; /* data packet */
    return CH_OK;
}

uint32_t Packet_AddData(Packet_t *pkt, uint8_t *buf, uint16_t len)
{
    /* add item content into buffer */
    memcpy((pkt->buf + pkt->ofs), buf, len);
    pkt->ofs += len;
    return CH_OK;
}

uint32_t Packet_Final(Packet_t *pkt)
{

    pkt->payload_len = pkt->ofs -6;
    pkt->len = pkt->ofs;

    pkt->buf[2] = (pkt->payload_len & 0x00FF)>>0;
    pkt->buf[3] = (pkt->payload_len & 0xFF00)>>8;

    /* crc */
    uint16_t crc;
    crc = 0;
    crc16_update(&crc, &pkt->buf[0], 4);
    crc16_update(&crc, &pkt->buf[6], pkt->payload_len);
    pkt->buf[4] = (crc & 0x00FF)>>0;
    pkt->buf[5] = (crc & 0xFF00)>>8;

    return CH_OK;
}

enum status
{
    kStatus_Idle,
    kStatus_Cmd,
    kStatus_LenLow,
    kStatus_LenHigh,
    kStatus_CRCLow,
    kStatus_CRCHigh,
    kStatus_Data,
};

Packet_t RxPkt;

static void OnDataReceived(Packet_t *pkt)
{
//	static uint32_t recv_cnt = 0;
//	if(recv_cnt++%100==0)
//		GPIO_ToggleBits(GPIOB, GPIO_Pin_14);
    int offset = 0;
    uint8_t *p = pkt->buf;
    while(offset < pkt->payload_len)
    {
        switch(p[offset])
        {
            case kItemID:
                id = p[1];
                offset += 2;
                break;
            case kItemAccRaw:
            case kItemAccCalibrated:
            case kItemAccFiltered:
            case kItemAccLinear:
                memcpy(acc, p + offset + 1, sizeof(acc));
                offset += 7;
                break;
            case kItemGyoRaw:
            case kItemGyoCalibrated:
            case kItemGyoFiltered:
                memcpy(gyo, p + offset + 1, sizeof(gyo));
                offset += 7;
                break;
            case kItemMagRaw:
            case kItemMagCalibrated:
            case kItemMagFiltered:
                memcpy(mag, p + offset + 1, sizeof(mag));
                offset += 7;
                break;
            case kItemRotationEular:
                eular[0] = ((float)(int16_t)(p[offset+1] + (p[offset+2]<<8)))/100;
                eular[1] = ((float)(int16_t)(p[offset+3] + (p[offset+4]<<8)))/100;
                eular[2] = ((float)(int16_t)(p[offset+5] + (p[offset+6]<<8)))/10;
                memcpy(eulars, p+offset+1, 6);
                offset += 7;
                break;
            case kItemRotationEular2:
                memcpy(eular, p + offset + 1, sizeof(eular));
                offset += 13;
                break;
            case kItemRotationQuat:
                memcpy(quat, p + offset + 1, sizeof(quat));
                offset += 17;
                break;
            case kItemPressure:
                offset += 5;
                break;
            case kItemTemperature:
                offset += 5;
                break;
            default:
				offset++;
                break;
        }
    }
}

uint32_t Packet_Decode(uint8_t c)
{
    static uint16_t CRCReceived = 0;            /* CRC value received from a frame */
    static uint16_t CRCCalculated = 0;          /* CRC value caluated from a frame */
    static uint8_t status = kStatus_Idle;       /* state machine */
    static uint8_t crc_header[4] = {0x5A, 0xA5, 0x00, 0x00};

    switch(status)
    {
        case kStatus_Idle:
            if(c == 0x5A)
                status = kStatus_Cmd;
            break;
        case kStatus_Cmd:
            RxPkt.type = c;
            switch(RxPkt.type)
            {
                case 0xA5:  /* Data */
                    status = kStatus_LenLow;
                    break;
                case 0xA6:  /* Ping */
                	OnDataReceived(&RxPkt);
                    status = kStatus_Idle;
                    break;
                case 0xA7:  /* Ping Respond */
                    RxPkt.ofs = 0;
                    status = kStatus_Data;
                    break;
            }
            break;
        case kStatus_LenLow:
            RxPkt.payload_len = c;
            crc_header[2] = c;
            status = kStatus_LenHigh;
            break;
        case kStatus_LenHigh:
            RxPkt.payload_len |= (c<<8);
            crc_header[3] = c;
            status = kStatus_CRCLow;
            break;
        case kStatus_CRCLow:
            CRCReceived = c;
            status = kStatus_CRCHigh;
            break;
        case kStatus_CRCHigh:
            CRCReceived |= (c<<8);
            RxPkt.ofs = 0;
            CRCCalculated = 0;
            status = kStatus_Data;
            break;
        case kStatus_Data:
            RxPkt.buf[RxPkt.ofs++] = c;
            if(RxPkt.type == 0xA7 && RxPkt.ofs >= 8)
            {
                RxPkt.payload_len = 8;
                OnDataReceived(&RxPkt);
                status = kStatus_Idle;
            }

            if(RxPkt.ofs >= RxPkt.payload_len && RxPkt.type == 0xA5)
            {
                /* calculate CRC */
                crc16_update(&CRCCalculated, crc_header, 4);
                crc16_update(&CRCCalculated, RxPkt.buf, RxPkt.ofs);

                /* CRC match */
                if(CRCCalculated == CRCReceived)
                {
                	OnDataReceived(&RxPkt);
                }
                status = kStatus_Idle;
            }
            break;
        default:
            status = kStatus_Idle;
            break;
    }
    return CH_OK;
}

void IMU_Decode()
{
	uint16_t i;
	for(i=0; i<imu_recv_len; i++)
	{
		Packet_Decode(RxPkt.buf[i]);
	}
}

int get_raw_acc(int16_t* a)
{
    memcpy(a, acc, sizeof(acc));
    return 0;
}

int get_raw_gyo(int16_t* g)
{
    memcpy(g, gyo, sizeof(gyo));
    return 0;
}

int get_raw_mag(int16_t* m)
{
    memcpy(m, mag, sizeof(mag));
    return 0;
}

int get_eular(float* e)
{
	memcpy(e, eulars, sizeof(eular));
    return 0;
}

int get_eulars(uint8_t *e)
{
	memcpy(e, eulars, 6);
	return 0;
}

int get_quat(float* q)
{
    memcpy(q, quat, sizeof(quat));
    return 0;
}

int get_id(uint8_t *user_id)
{
    *user_id = id;
    return 0;
}

